import org.apache.commons.math.linear.Array2DRowRealMatrix;
import org.apache.commons.math.linear.RealMatrix;

import edu.wsu.KheperaSimulator.RobotController;

/**
 * 
 * The Controller3c class. The robot pushes a ball and balances it. Proportional
 * controller
 * 
 * @author Philipp Rusch und Markus Wetzel
 */
public class Controller3c extends RobotController {

	// Sensoren:
	// 0 .. Links
	// 1 .. Links Vorne Schraeg
	// 2 .. links Vorne
	// 3 .. Rechts Vorne
	// 4 .. Rechts Vorne Schraeg
	// 5 .. Rechts
	// 6 .. Rechts Hinten
	// 7 .. Links Hinten

	double[][] controllerMatrixData = { { 0d, 0d, 0d, 1d, 1d, 1d, 0d, 0d },
			{ 1d, 1d, 1d, 0d, 0d, 0d, 0d, 0d } };
	RealMatrix controllerMatrix = new Array2DRowRealMatrix(controllerMatrixData);

	@Override
	public void close() throws Exception {
	}

	@Override
	public void doWork() throws Exception {
		int sumSensorValues = 0;
		for (int i = 0; i < 8; i++) {
			sumSensorValues += getDistanceValue(i);
		}

		sumSensorValues = sumSensorValues / 9;

		System.out.println("sumSensorValues: " + sumSensorValues);
		System.out.println("Sensor 0: " + getLightValue(0));
		System.out.println("Matrix 0: " + 1 / getLightValue(0)
				* sumSensorValues);

		if (sumSensorValues != 0) {
			double[][] sensorVectorData = {
					{ getDistanceValue(0) / sumSensorValues },
					{ getDistanceValue(1) / sumSensorValues },
					{ getDistanceValue(2) / sumSensorValues },
					{ getDistanceValue(3) / sumSensorValues },
					{ getDistanceValue(4) / sumSensorValues },
					{ getDistanceValue(5) / sumSensorValues },
					{ getDistanceValue(6) / sumSensorValues },
					{ getDistanceValue(7) / sumSensorValues } };
			RealMatrix sensorVector = new Array2DRowRealMatrix(sensorVectorData);

			// Now multiply controllerMatrix by sensorMatrix
			RealMatrix motorVector = controllerMatrix.multiply(sensorVector);

			System.out.println(motorVector.getEntry(0, 0) + ","
					+ motorVector.getEntry(1, 0));

			setMotorSpeeds((int) motorVector.getEntry(0, 0),
					(int) motorVector.getEntry(1, 0));
		}
	}
}
